﻿using System;
using System.Collections.Generic;
using System.Configuration;
using System.Diagnostics;
using System.Drawing;
using Imaging = System.Drawing.Imaging;
using System.IO;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using System.Xml;
using OpenTK;
using OpenTK.Graphics;
using OpenTK.Graphics.OpenGL;


namespace LasRendering
{
    public static class VertexColorHelper
    {
        private class ImageCalibratedInfo
        {
            public string Path;
            public Matrix4 ProjectionMatrix;
            public Vector3 CameraPosition;
            public int ImageWidth;
            public int ImageHeight;
            public ManualResetEvent Event;
        }

        private static ImageCalibratedInfo[] imageList;

        private static float vertexAvgSize;
        private static int[] maxViewport;
        private static IntPtr vpImagery;
        private static IntPtr fpImagery;

        private static Semaphore imageSemaphore;
        private static int maxNumOfCpuVertices;
        private static int maxNumOfGpuVertices;
        private static int numConcurrentProcessingImages;
        private static List<QuadTreeNode> cpuMemCache = new List<QuadTreeNode>();
        private static List<QuadTreeNode> gpuVboCache = new List<QuadTreeNode>();
        private static int numOfVerticesInCpuCache = 0;
        private static int numOfVerticesInGpuCache = 0;
        private static object cacheLock = new object();

        private static long totalMillisecondsForOpenGl = 0;
        private static long totalMillisecondsForDrawingVertices = 0;
        private static long totalMillisecondsForMappingColor = 0;
        private static long totalMillisecondsForLoadingImage = 0;

        public static bool Init(Vector3 offset)
        {
            string calibXmlPath = ConfigurationManager.AppSettings["imageCalibXmlPath"];
            if (string.IsNullOrEmpty(calibXmlPath))
            {
                return false;
            }

            XmlDocument calibXml = new XmlDocument();
            calibXml.Load(calibXmlPath);
            var cameraCalibXmlNodes = calibXml.SelectNodes("/CameraCalibsData/CameraCalibs/CameraCalib");
            imageList = new ImageCalibratedInfo[cameraCalibXmlNodes.Count];
            for (int i = 0; i < cameraCalibXmlNodes.Count; i++)
            {
                var cameraCalibXmlElement = cameraCalibXmlNodes[i] as XmlElement;
                var imageInfo = new ImageCalibratedInfo();
                imageInfo.Path = cameraCalibXmlElement.Attributes["ImageFileName"].Value;

                var m4 = MatrixHelper.Parse(cameraCalibXmlElement.Attributes["ProjectiveMatrix"].Value);
                m4.M44 = 1;
                var offsetMatrix = Matrix4.Identity;
                offsetMatrix.M14 = offset.X;
                offsetMatrix.M24 = offset.Y;
                offsetMatrix.M34 = offset.Z;
                imageInfo.ProjectionMatrix = Matrix4.Mult(m4, offsetMatrix);

                m4 = new Matrix4(
                    imageInfo.ProjectionMatrix.M11, imageInfo.ProjectionMatrix.M12, imageInfo.ProjectionMatrix.M13, 0,
                    imageInfo.ProjectionMatrix.M21, imageInfo.ProjectionMatrix.M22, imageInfo.ProjectionMatrix.M23, 0,
                    imageInfo.ProjectionMatrix.M31, imageInfo.ProjectionMatrix.M32, imageInfo.ProjectionMatrix.M33, 0,
                    0, 0, 0, 1);
                m4.Invert();
                var m3 = new Matrix3x3(
                    m4.M11, m4.M12, m4.M13,
                    m4.M21, m4.M22, m4.M23,
                    m4.M31, m4.M32, m4.M33);
                var v = new Vector3(imageInfo.ProjectionMatrix.M14, imageInfo.ProjectionMatrix.M24, imageInfo.ProjectionMatrix.M34);
                imageInfo.CameraPosition = m3 * v;
                if (imageInfo.CameraPosition.Z < 0)
                {
                    imageInfo.CameraPosition = -imageInfo.CameraPosition;
                }

                imageList[i] = imageInfo;
            }

            int index = 0;
            while (index < imageList.Length)
            {
                var events = new ManualResetEvent[Math.Min(imageList.Length - index, 64)];
                for (int i = 0; index < imageList.Length && i < 64; index++, i++)
                {
                    var @event = new ManualResetEvent(false);
                    events[i] = @event;
#if DEBUG
                    GetImageSize(new object[] { imageList[i], @event });
#else
                    ThreadPool.QueueUserWorkItem(GetImageSize, new object[] { imageList[index], @event });
#endif
                }
                WaitHandle.WaitAll(events);
                for (int i = 0; i < events.Length; i++)
                {
                    events[i].Close();
                }
            }

            Array.Sort(imageList, delegate(ImageCalibratedInfo info1, ImageCalibratedInfo info2)
            {
                return string.Compare(info1.Path, info2.Path, true);
            });

            return true;
        }

        private static void GetImageSize(object state)
        {
            var args = (object[])state;
            var imageInfo = (ImageCalibratedInfo)args[0];
            var @event = (ManualResetEvent)args[1];

            var size = ImageHelper.GetDimensions(imageInfo.Path);
            imageInfo.ImageWidth = size.Width;
            imageInfo.ImageHeight = size.Height;

            @event.Set();
        }

        public static void PreprocessFindColor(string filename, QuadTree vertexQuadTree)
        {
            if (imageList == null)
            {
                return;
            }

            var maxConcurrentImages = Convert.ToInt32(ConfigurationManager.AppSettings["maxConcurrentImagesForColorPreprocessing"]);
            imageSemaphore = new Semaphore(maxConcurrentImages, maxConcurrentImages);
            maxNumOfCpuVertices = Convert.ToInt32(ConfigurationManager.AppSettings["maxNumOfCpuVerticesForColorPreprocessing"]);
            maxNumOfGpuVertices = Convert.ToInt32(ConfigurationManager.AppSettings["maxNumOfGpuVerticesForColorPreprocessing"]);
            //float pointSizeScale = Convert.ToSingle(ConfigurationManager.AppSettings["pointSizeScale"]);
            vertexAvgSize = vertexQuadTree.VertexAvgSize;// *pointSizeScale;

            InitOpenGl();

            for (int i = 0; i < imageList.Length; i++)
            {
                FindColorOnImage(imageList[i], vertexQuadTree);
            }

            int index = 0;
            while (index < imageList.Length)
            {
                var events = new ManualResetEvent[Math.Min(imageList.Length - index, 64)];
                for (int i = 0; i < events.Length; i++, index++)
                {
                    events[i] = imageList[index].Event;
                }
                WaitHandle.WaitAll(events);
            }
            for (int i = 0; i < imageList.Length; i++)
            {
                imageList[i].Event.Close();
                imageList[i].Event = null;
            }

            imageSemaphore.Close();
            UninitOpenGl();

            LasHelper.FinishSavingColor();

            Log.Write("Total ms for opengl: {0}", totalMillisecondsForOpenGl);
            Log.Write("Total ms for drawing vertices: {0}", totalMillisecondsForDrawingVertices);
            Log.Write("Total ms for mapping color: {0}", totalMillisecondsForMappingColor);
            Log.Write("Total ms for loading image: {0}", totalMillisecondsForLoadingImage);
        }

        private static void InitOpenGl()
        {
            int maxNumMrtTextures;
            GL.GetInteger(GetPName.MaxColorAttachmentsExt, out maxNumMrtTextures);
            Debug.Assert(maxNumMrtTextures >= 2);

            maxViewport = new int[2];
            GL.GetInteger(GetPName.MaxViewportDims, maxViewport);

            GL.ClearColor(0, 0, 0, 0);
            GL.Enable(EnableCap.PointSprite);
            GL.Enable(EnableCap.VertexProgramPointSize);
            GL.Enable(EnableCap.DepthTest);
            GL.DepthFunc(DepthFunction.Lequal);
            GL.DepthMask(true);

            ShaderHelper.InitCg();
            vpImagery = ShaderHelper.LoadProgram(ShaderHelper.VertexProfile, "vpImagery.cg");
            fpImagery = ShaderHelper.LoadProgram(ShaderHelper.FragmentProfile, "fpImagery.cg");
        }

        private static void UninitOpenGl()
        {
            while (gpuVboCache.Count > 0)
            {
                FreeGpuVbo(gpuVboCache[0], 0);
            }
            while (cpuMemCache.Count > 0)
            {
                FreeCpuMemory(cpuMemCache[0], 0);
            }
            while (colorSavingList.Count > 0)
            {
                Thread.Sleep(50);
            }

            ShaderHelper.UnloadProgram(vpImagery);
            ShaderHelper.UnloadProgram(fpImagery);
        }

        private static void FindColorOnImage(ImageCalibratedInfo imageInfo, QuadTree vertexQuadTree)
        {
            imageSemaphore.WaitOne();
            Interlocked.Increment(ref numConcurrentProcessingImages);

            Stopwatch stopwatch = new Stopwatch();
            stopwatch.Start();

            Vector3 eyePosition = new Vector3(imageInfo.CameraPosition.X, imageInfo.CameraPosition.Z, -imageInfo.CameraPosition.Y);
            var m = imageInfo.ProjectionMatrix;
            Matrix4 modelViewProjMatrix = new Matrix4(
                m.M11, m.M13, -m.M12, m.M14,
                m.M21, m.M23, -m.M22, m.M24,
                m.M31, m.M33, -m.M32, m.M34,
                0, 0, 0, 0);
            float vertexSizeCoefficient = (float)(imageInfo.ImageHeight * vertexAvgSize / (2 * Math.Tan(MathHelper.DegreesToRadians(50 / 2))));
            float nearCull = Math.Max(1, eyePosition.Y - vertexQuadTree.MaxPosition.Y);
            float farCull = nearCull + (vertexQuadTree.MaxPosition - vertexQuadTree.MinPosition).Length;

            int mrtFboId;
            DrawBuffersEnum[] mrtTextures = { (DrawBuffersEnum)FramebufferAttachment.ColorAttachment0Ext, (DrawBuffersEnum)FramebufferAttachment.ColorAttachment1Ext };
            int mrtIndexTextureId;
            int mrtWeightTextureId;
            int mrtDepthTextureId;

            Debug.Assert(imageInfo.ImageWidth <= maxViewport[0]);
            Debug.Assert(imageInfo.ImageHeight <= maxViewport[1]);
            GL.Viewport(0, 0, imageInfo.ImageWidth, imageInfo.ImageHeight);

            // Create an FBO
            GL.Ext.GenFramebuffers(1, out mrtFboId);

            // Create MRT textures
            GL.GenTextures(1, out mrtIndexTextureId);
            GL.BindTexture(TextureTarget.Texture2D, mrtIndexTextureId);
            GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMinFilter, (int)TextureMinFilter.Nearest);
            GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMagFilter, (int)TextureMagFilter.Nearest);
            GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.Rgba8, imageInfo.ImageWidth, imageInfo.ImageHeight, 0, PixelFormat.Rgba, PixelType.Byte, IntPtr.Zero);
            GL.BindTexture(TextureTarget.Texture2D, 0);
            GL.GenTextures(1, out mrtWeightTextureId);
            GL.BindTexture(TextureTarget.Texture2D, mrtWeightTextureId);
            GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMinFilter, (int)TextureMinFilter.Nearest);
            GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMagFilter, (int)TextureMagFilter.Nearest);
            GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.Alpha8, imageInfo.ImageWidth, imageInfo.ImageHeight, 0, PixelFormat.Rgba, PixelType.Byte, IntPtr.Zero);
            GL.BindTexture(TextureTarget.Texture2D, 0);
            GL.GenTextures(1, out mrtDepthTextureId);
            GL.BindTexture(TextureTarget.Texture2D, mrtDepthTextureId);
            GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMinFilter, (int)TextureMinFilter.Nearest);
            GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.TextureMagFilter, (int)TextureMagFilter.Nearest);
            GL.TexParameter(TextureTarget.Texture2D, TextureParameterName.DepthTextureMode, (int)All.Intensity);
            GL.TexImage2D(TextureTarget.Texture2D, 0, PixelInternalFormat.DepthComponent24, imageInfo.ImageWidth, imageInfo.ImageHeight, 0, PixelFormat.DepthComponent, PixelType.Float, IntPtr.Zero);
            GL.BindTexture(TextureTarget.Texture2D, 0);

            // Bind the FBO and attach color texture to it
            GL.Ext.BindFramebuffer(FramebufferTarget.FramebufferExt, mrtFboId);
            GL.Ext.FramebufferTexture2D(FramebufferTarget.FramebufferExt, (FramebufferAttachment)mrtTextures[0], TextureTarget.Texture2D, mrtIndexTextureId, 0);
            GL.Ext.FramebufferTexture2D(FramebufferTarget.FramebufferExt, (FramebufferAttachment)mrtTextures[1], TextureTarget.Texture2D, mrtWeightTextureId, 0);
            GL.Ext.FramebufferTexture2D(FramebufferTarget.FramebufferExt, FramebufferAttachment.DepthAttachmentExt, TextureTarget.Texture2D, mrtDepthTextureId, 0);

            GL.DrawBuffers(mrtTextures.Length, mrtTextures);

            GL.MatrixMode(MatrixMode.Modelview);
            GL.LoadIdentity();
            GL.MatrixMode(MatrixMode.Projection);
            GL.LoadIdentity();

            GL.Clear(ClearBufferMask.ColorBufferBit | ClearBufferMask.DepthBufferBit);

            ShaderHelper.SetParameter(vpImagery, "eyePosition", eyePosition);
            ShaderHelper.SetParameter(vpImagery, "sizeCoefficient", vertexSizeCoefficient);
            ShaderHelper.SetParameter(vpImagery, "nearCull", nearCull);
            ShaderHelper.SetParameter(vpImagery, "farNearDistance", farCull - nearCull);
            ShaderHelper.SetParameter(vpImagery, "modelViewProjRow0", modelViewProjMatrix.Row0);
            ShaderHelper.SetParameter(vpImagery, "modelViewProjRow1", modelViewProjMatrix.Row1);
            ShaderHelper.SetParameter(vpImagery, "modelViewProjRow2", modelViewProjMatrix.Row2);
            ShaderHelper.SetParameter(vpImagery, "imageWidth", imageInfo.ImageWidth);
            ShaderHelper.SetParameter(vpImagery, "imageHeight", imageInfo.ImageHeight);

            ShaderHelper.BindProgram(ShaderHelper.VertexProfile, vpImagery);
            ShaderHelper.BindProgram(ShaderHelper.FragmentProfile, fpImagery);

            stopwatch.Stop();
            Interlocked.Add(ref totalMillisecondsForOpenGl, stopwatch.ElapsedMilliseconds);

            var visibleNodeList = DrawVertices(vertexQuadTree, modelViewProjMatrix, imageInfo.ImageWidth, imageInfo.ImageHeight);

            stopwatch.Reset();
            stopwatch.Start();

            ShaderHelper.UnbindProgram(ShaderHelper.VertexProfile);
            ShaderHelper.UnbindProgram(ShaderHelper.FragmentProfile);

            GL.Flush();

            foreach (var node in visibleNodeList)
            {
                Interlocked.Decrement(ref node.GpuLockCount);
            }

            IntPtr indexScan0 = Marshal.AllocHGlobal(imageInfo.ImageWidth * imageInfo.ImageHeight * sizeof(int));
            GL.Ext.FramebufferReadBuffer(mrtFboId, (ReadBufferMode)FramebufferAttachment.ColorAttachment0Ext);
            GL.ReadPixels(0, 0, imageInfo.ImageWidth, imageInfo.ImageHeight, PixelFormat.Bgra, PixelType.UnsignedByte, indexScan0);

            IntPtr weightScan0 = Marshal.AllocHGlobal(imageInfo.ImageWidth * imageInfo.ImageHeight);
            GL.Ext.FramebufferReadBuffer(mrtFboId, (ReadBufferMode)FramebufferAttachment.ColorAttachment1Ext);
            GL.ReadPixels(0, 0, imageInfo.ImageWidth, imageInfo.ImageHeight, PixelFormat.Alpha, PixelType.UnsignedByte, weightScan0);

            GL.DeleteTextures(1, ref mrtIndexTextureId);
            GL.DeleteTextures(1, ref mrtWeightTextureId);
            GL.DeleteTextures(1, ref mrtDepthTextureId);
            GL.Ext.DeleteFramebuffers(1, ref mrtFboId);

            GL.Ext.BindFramebuffer(FramebufferTarget.FramebufferExt, 0);

            stopwatch.Stop();
            Interlocked.Add(ref totalMillisecondsForOpenGl, stopwatch.ElapsedMilliseconds);

            imageInfo.Event = new ManualResetEvent(false);
            ThreadPool.QueueUserWorkItem(MapColorOnImage, new object[] { imageInfo, indexScan0, weightScan0, visibleNodeList });
        }

        private static List<QuadTreeNode> DrawVertices(QuadTree vertexQuadTree, Matrix4 modelViewProjMatrix, int imageWidth, int imageHeight)
        {
            Stopwatch stopwatch = new Stopwatch();
            stopwatch.Start();

            float halfY = (vertexQuadTree.MinPosition.Y + vertexQuadTree.MaxPosition.Y) / 2;
            var visibleNodeList = new List<QuadTreeNode>();
            vertexQuadTree.EnumerateVisibleNode(
                delegate(Vector2 position)
                {
                    Vector4 p = new Vector4(position.X, halfY, -position.Y, 1);
                    float w = Vector4.Dot(modelViewProjMatrix.Row2, p);
                    int u = (int)(Vector4.Dot(modelViewProjMatrix.Row0, p) / w);
                    int v = (int)(Vector4.Dot(modelViewProjMatrix.Row1, p) / w);
                    return new Vector2((float)u / imageWidth, (float)v / imageHeight);
                },
                delegate(QuadTreeNode node, Vector2 uv0, Vector2 uv1, Vector2 uv2, Vector2 uv3)
                {
                    visibleNodeList.Add(node);
                },
                null);

            int memRequired = 0;
            lock (cacheLock)
            {
                foreach (var node in visibleNodeList)
                {
                    if (node.IndexVertexBucket == null || node.IndexVertexBucket.Ptr == IntPtr.Zero)
                    {
                        memRequired += node.VertexIndexLink.Count;
                    }
                    Interlocked.Increment(ref node.MemLockCount);
                    Interlocked.Increment(ref node.GpuLockCount);
                }
            }
            FreeCpuMemoryFifo(memRequired);

            List<ManualResetEvent> eventList = new List<ManualResetEvent>();
            lock (cacheLock)
            {
                foreach (var node in visibleNodeList)
                {
                    if (node.IndexVertexBucket == null || node.ColorList == null)
                    {
                        var @event = new ManualResetEvent(false);
                        eventList.Add(@event);
                        ThreadPool.QueueUserWorkItem(LoadFromDiskToCpuMemory, new object[] { node, @event });
                    }
                }
            }
            int index = 0;
            while (index < eventList.Count)
            {
                var events = new ManualResetEvent[Math.Min(eventList.Count - index, 64)];
                for (int i = 0; i < events.Length; i++, index++)
                {
                    events[i] = eventList[index];
                }
                WaitHandle.WaitAll(events);
            }
            for (int i = 0; i < eventList.Count; i++)
            {
                eventList[i].Close();
            }

            int gpuRequired = 0;
            foreach (var node in visibleNodeList)
            {
                if (node.IndexVertexBucket == null || node.IndexVertexBucket.VertexBufferObjectId == 0)
                {
                    gpuRequired += node.VertexIndexLink.Count;
                }
            }
            FreeGpuVboFifo(gpuRequired);

            foreach (var node in visibleNodeList)
            {
                var vertexBucket = node.IndexVertexBucket;

                if (vertexBucket == null)
                {
                    continue;
                }

                if (vertexBucket.VertexBufferObjectId == 0)
                {
                    LoadFromCpuMemoryToGpuVbo(node);
                }

                if (vertexBucket.VertexBufferObjectId > 0)
                {
                    GL.BindBuffer(BufferTarget.ArrayBuffer, vertexBucket.VertexBufferObjectId);
                    GL.InterleavedArrays(InterleavedArrayFormat.C4fN3fV3f, vertexBucket.BytesPerVertex, IntPtr.Zero);
                    GL.DrawArrays(BeginMode.Points, 0, vertexBucket.CountInVBO);
                    GL.BindBuffer(BufferTarget.ArrayBuffer, 0);
                }
            }

            stopwatch.Stop();
            Interlocked.Add(ref totalMillisecondsForDrawingVertices, stopwatch.ElapsedMilliseconds);

            return visibleNodeList;
        }

        private static bool IsPositionVisible(Vector3 position, Matrix4 matrix, int imageWidth, int imageHeight)
        {
            Vector4 p = new Vector4(position, 1);
            float w = Vector4.Dot(matrix.Row2, p);
            int u = (int)(Vector4.Dot(matrix.Row0, p) / w);
            int v = (int)(Vector4.Dot(matrix.Row1, p) / w);
            return (u > 0 && v > 0 && u < imageWidth && v < imageHeight);
        }

        private static void MapColorOnImage(object state)
        {
            var args = (object[])state;
            var imageInfo = (ImageCalibratedInfo)args[0];
            var indexScan0 = (IntPtr)args[1];
            var weightScan0 = (IntPtr)args[2];
            var visibleNodeList = (List<QuadTreeNode>)args[3];

#if DEBUG
            var tempFilename = Path.GetFileNameWithoutExtension(imageInfo.Path);
            using (var imageSave = new Bitmap(imageInfo.ImageWidth, imageInfo.ImageHeight, imageInfo.ImageWidth * sizeof(int), Imaging.PixelFormat.Format32bppArgb, indexScan0))
            {
                imageSave.Save(string.Format(@"d:\temp\usc\{0}-0.png", tempFilename));
            }
            using (var imageSave = new Bitmap(imageInfo.ImageWidth, imageInfo.ImageHeight, Imaging.PixelFormat.Format32bppArgb))
            {
                var imageData = imageSave.LockBits(new Rectangle(0, 0, imageSave.Width, imageSave.Height), Imaging.ImageLockMode.ReadWrite, Imaging.PixelFormat.Format32bppArgb);
                int count = imageData.Width * imageData.Height;
                for (int i = 0; i < count; i++)
                {
                    byte weight = Marshal.ReadByte(weightScan0, i);
                    Color c = Color.FromArgb(weight, weight, weight);
                    Marshal.WriteInt32(imageData.Scan0, i * 4, c.ToArgb());
                }
                imageSave.UnlockBits(imageData);
                imageSave.Save(string.Format(@"d:\temp\usc\{0}-1.png", tempFilename));
            }
#endif

            Stopwatch stopwatch = new Stopwatch();
            stopwatch.Start();
            using (var image = new Bitmap(imageInfo.Path))
            {
                stopwatch.Stop();
                Interlocked.Add(ref totalMillisecondsForLoadingImage, stopwatch.ElapsedMilliseconds);

                stopwatch.Reset();
                stopwatch.Start();

                var imageData = image.LockBits(new Rectangle(0, 0, image.Width, image.Height), Imaging.ImageLockMode.ReadOnly, Imaging.PixelFormat.Format32bppArgb);
                IntPtr colorScan0 = imageData.Scan0;
                int count = image.Width * image.Height;
                AddColorFuncHint hint = new AddColorFuncHint();
                hint.startIndex = int.MaxValue;
                for (int i = 0; i < count; i++)
                {
                    int offset = i * sizeof(int);
                    int index = Marshal.ReadInt32(indexScan0, offset);
                    if (index != 0)
                    {
                        index = ((byte.MaxValue - (index >> 24)) << 24) + (index & 0xFFFFFF);
                        float weight = (float)Marshal.ReadByte(weightScan0, i) / byte.MaxValue;
                        Color color = Color.FromArgb(Marshal.ReadInt32(colorScan0, offset));

                        AddColor(visibleNodeList, index, color, weight, ref hint);
                    }
                }
                image.UnlockBits(imageData);

                stopwatch.Stop();
                Interlocked.Add(ref totalMillisecondsForMappingColor, stopwatch.ElapsedMilliseconds);
            }

            Marshal.FreeHGlobal(indexScan0);
            Marshal.FreeHGlobal(weightScan0);

            Interlocked.Decrement(ref numConcurrentProcessingImages);

            foreach (var node in visibleNodeList)
            {
                Interlocked.Decrement(ref node.MemLockCount);
            }

            imageSemaphore.Release();
            imageInfo.Event.Set();
        }

        private struct AddColorFuncHint
        {
            public QuadTreeNode quadTreeNode;
            public int startIndex;
            public int endIndex;
            public int globalIndex;
            public int nodeIndex;
        }

        private static void AddColor(List<QuadTreeNode> visibleNodeList, int index, Color color, float weight, ref AddColorFuncHint hint)
        {
            QuadTreeNode nodeOfIndex = null;
            int indexOfNode = 0;

            if (index >= hint.startIndex && index <= hint.endIndex)
            {
                nodeOfIndex = hint.quadTreeNode;
                indexOfNode = hint.nodeIndex + (index - hint.globalIndex);
            }
            else
            {
                for (int i = 0; i < visibleNodeList.Count; i++)
                {
                    var node = visibleNodeList[i];
                    indexOfNode = 0;

                    var item = node.VertexIndexLink.First;
                    while (item != null)
                    {
                        if (index >= item.StartIndex && index <= item.EndIndex)
                        {
                            indexOfNode += index - item.StartIndex;
                            nodeOfIndex = node;

                            hint.quadTreeNode = node;
                            hint.startIndex = item.StartIndex;
                            hint.endIndex = item.EndIndex;
                            hint.globalIndex = index;
                            hint.nodeIndex = indexOfNode;

                            break;
                        }
                        indexOfNode += item.EndIndex - item.StartIndex + 1;
                        item = item.Next;
                    }

                    if (nodeOfIndex != null)
                    {
                        break;
                    }
                }
            }

            var colorList = nodeOfIndex.ColorList;
            colorList[indexOfNode].R += color.R * weight;
            colorList[indexOfNode].G += color.G * weight;
            colorList[indexOfNode].B += color.B * weight;
            colorList[indexOfNode].A += weight;
        }

        #region CPU cache, GPU cache

        private static void LoadFromDiskToCpuMemory(object state)
        {
            var args = (object[])state;
            var node = (QuadTreeNode)args[0];
            var @event = (ManualResetEvent)args[1];

            VertexBucket vertexBucket;
            Color4[] colorList;
            VertexHelper.CreateVertexIndexBucket(node, out vertexBucket, out colorList);
            if (vertexBucket.CountInMemory > 0)
            {
                lock (cacheLock)
                {
                    if (node.ColorList == null)
                    {
                        node.ColorList = colorList;
                    }
                    else
                    {
                        int index = colorSavingList.IndexOf(node);
                        if (index >= 0)
                        {
                            colorSavingList.RemoveAt(index);
                        }
                    }

                    if (node.IndexVertexBucket == null)
                    {
                        node.IndexVertexBucket = vertexBucket;
                        numOfVerticesInCpuCache += vertexBucket.CountInMemory;
                        cpuMemCache.Add(node);
                    }
                }
            }
            else
            {
                VertexHelper.FreeMemory(vertexBucket);
            }

            if (@event != null)
            {
                @event.Set();
            }
        }

        private static void LoadFromCpuMemoryToGpuVbo(QuadTreeNode node)
        {
            var vertexBucket = node.IndexVertexBucket;

            GL.GenBuffers(1, out vertexBucket.VertexBufferObjectId);
            GL.BindBuffer(BufferTarget.ArrayBuffer, vertexBucket.VertexBufferObjectId);
            GL.BufferData(BufferTarget.ArrayBuffer, new IntPtr(vertexBucket.BytesPerVertex * vertexBucket.CountInMemory), vertexBucket.Ptr, BufferUsageHint.StaticDraw);
            vertexBucket.CountInVBO = vertexBucket.CountInMemory;

            lock (cacheLock)
            {
                numOfVerticesInGpuCache += vertexBucket.CountInVBO;
                gpuVboCache.Add(node);
            }
        }

        private static void FreeCpuMemoryFifo(int memRequired)
        {
            int maxNum = maxNumOfCpuVertices - memRequired;
            lock (cacheLock)
            {
                int i = 0;
                while (i < cpuMemCache.Count)
                {
                    if (numOfVerticesInCpuCache < maxNum)
                    {
                        break;
                    }

                    var node = cpuMemCache[i];
                    if (node.MemLockCount == 0)
                    {
                        FreeCpuMemory(node, i);
                        continue;
                    }

                    i++;
                }
            }
        }

        private static void FreeGpuVboFifo(int gpuRequired)
        {
            int maxNum = maxNumOfGpuVertices - gpuRequired;
            lock (cacheLock)
            {
                int i = 0;
                while (i < gpuVboCache.Count)
                {
                    if (numOfVerticesInGpuCache < maxNum)
                    {
                        break;
                    }

                    var node = gpuVboCache[i];
                    if (node.GpuLockCount == 0)
                    {
                        FreeGpuVbo(node, i);
                        continue;
                    }

                    i++;
                }
            }
        }

        private static void FreeCpuMemory(QuadTreeNode node, int indexHint)
        {
            lock (cacheLock)
            {
                if (node.MemLockCount != 0)
                {
                    return;
                }

                var colorList = node.ColorList;
                if (colorList != null)
                {
                    colorSavingList.Add(node);
                    ThreadPool.QueueUserWorkItem(SaveColor, new object[] { node });
                }

                var vertexBucket = node.IndexVertexBucket;
                if (vertexBucket != null)
                {
                    if (vertexBucket.Ptr != IntPtr.Zero)
                    {
                        numOfVerticesInCpuCache -= vertexBucket.CountInMemory;
                        VertexHelper.FreeMemory(vertexBucket);
                        vertexBucket.Ptr = IntPtr.Zero;

                        if (cpuMemCache[indexHint] == node)
                        {
                            cpuMemCache.RemoveAt(indexHint);
                        }
                        else
                        {
                            cpuMemCache.Remove(node);
                        }
                    }

                    if (vertexBucket.VertexBufferObjectId == 0)
                    {
                        node.IndexVertexBucket = null;
                    }
                }
            }
        }

        private static void FreeGpuVbo(QuadTreeNode node, int indexHint)
        {
            lock (cacheLock)
            {
                if (node.GpuLockCount != 0)
                {
                    return;
                }

                var vertexBucket = node.IndexVertexBucket;
                if (vertexBucket == null)
                {
                    return;
                }

                if (vertexBucket.VertexBufferObjectId > 0)
                {
                    numOfVerticesInGpuCache -= vertexBucket.CountInVBO;
                    GL.DeleteBuffers(1, ref vertexBucket.VertexBufferObjectId);
                    vertexBucket.VertexBufferObjectId = 0;
                    if (gpuVboCache[indexHint] == node)
                    {
                        gpuVboCache.RemoveAt(indexHint);
                    }
                    else
                    {
                        gpuVboCache.Remove(node);
                    }
                }

                if (vertexBucket.Ptr == IntPtr.Zero)
                {
                    node.IndexVertexBucket = null;
                }
            }
        }

        private static List<QuadTreeNode> colorSavingList = new List<QuadTreeNode>();

        private static void SaveColor(object state)
        {
            var args = (object[])state;
            var node = (QuadTreeNode)args[0];
            var colorList = node.ColorList;

            LasHelper.SaveColor(node, colorList);

            lock (cacheLock)
            {
                node.ColorList = null;
                colorSavingList.Remove(node);
            }
        }

        #endregion
    }
}
